#!/usr/bin/env python
#-*- coding: utf-8 -*-

import gobject
import struct 
import numpy as np

class ApInput():
    def __init__(self):
        self.throttle     = 0.
        self.stick_roll   = 0.
        self.stick_pitch  = 0.
        self.stick_yaw    = 0.

#
# http://www.jezra.net/blog/Python_Joystick_Class_using_Gobject
# For documentation on Linux Joystick programming please see 
# http://www.mjmwired.net/kernel/Documentation/input/joystick-api.txt 
#
class Joystick(gobject.GObject, ApInput):
    EVENT_FORMAT = "IhBB" 
    EVENT_SIZE = struct.calcsize(EVENT_FORMAT)
    EVENT_AXIS = 0x02
    EVENT_INIT = 0x80

    AXIS_THROTTLE = 0
    AXIS_ROLL     = 3
    AXIS_PITCH    = 2
    AXIS_YAW      = 1
    AXIS_NB       = 8
    
    def __init__(self, dev_str=None):
        gobject.GObject.__init__(self)
        ApInput.__init__(self)
        self.values = np.zeros(Joystick.AXIS_NB)
        if dev_str <> None:
            try:
                self._device = open(dev_str)
            except IOError as e:
                print "Warning: Could not open joystick ({0}: {1})".format(dev_str, e.strerror)
            else:
                gobject.io_add_watch(self._device,gobject.IO_IN,self.read_joystick_data)
            
    def read_joystick_data(self, source, condition):
        buf = self._device.read(Joystick.EVENT_SIZE)
        time, value, type, number = struct.unpack(Joystick.EVENT_FORMAT, buf)
#        print "js: {:d} {:d} {:d} {:d}".format(time, value, type, number)
        if type == Joystick.EVENT_AXIS:
#            print "axis: {:d} value: {:d}".format(number, value)
            self.values[number] = value

            self.stick_roll  =  self.values[Joystick.AXIS_ROLL]     / 32000.
            self.stick_pitch = -self.values[Joystick.AXIS_PITCH]    / 32000.
            self.stick_yaw   =  self.values[Joystick.AXIS_YAW]      / 32000.
            self.throttle    = -self.values[Joystick.AXIS_THROTTLE] / 32000.
        return True 


#
# This is a test program that displays joystick input
#
import gtk
if __name__ == "__main__":

    def on_key_down(self, event):
        print "key_down"

    def periodic_callback(js):
        print "stick {: .02f} {: .02f} {: .02f} {: 0.2f}\r".format(js.stick_roll, js.stick_pitch, js.stick_yaw, js.throttle)
        return True

    window = gtk.Window(gtk.WINDOW_TOPLEVEL)
    window.connect("key-press-event", on_key_down)
    vbox = gtk.VBox()
    window.add(vbox)
    vbox.pack_start(gtk.Label('time: 0.00 s'), expand=False, fill=False)
    js = Joystick("/dev/input/js1")
    window.show_all()
    gobject.timeout_add(1000/25, periodic_callback, js)
    gtk.main()
